package com.igo.util;

import com.igo.domain.datacollection.devicedata.FlatCarData;

import java.util.ArrayList;
import java.util.List;

public class StrictGpsFilter {

    // 地球半径（米）
    private static final double EARTH_RADIUS = 6371000;
    // 最大允许速度（4km/h转换为m/s）
    private static final double MAX_SPEED = 5.0 * 1000 / 3600; // ≈1.111 m/s
    // 1分钟最大理论位移（严格模式不加缓冲）
    private static final double MAX_DISPLACEMENT = MAX_SPEED * 60; // 66.667米
    // 冷启动最大允许位移（设备重启后的第一个点）
    private static final double COLD_START_MAX_DISPLACEMENT = 100.0; // 100米
    // 冷启动时间阈值（5分钟以上视为冷启动）
    private static final long COLD_START_THRESHOLD = 15 * 60 * 1000; // 15分钟
    
    /**
     * 严格过滤所有漂移点
     * @param points 原始GPS点列表（必须按时间排序）
     * @return 完全剔除漂移点后的列表
     */
    public static List<FlatCarData> filterStrictly(List<FlatCarData> points) {
        if (points == null || points.size() <= 1) {
            return points != null ? new ArrayList<>(points) : new ArrayList<>();
        }

        List<FlatCarData> filtered = new ArrayList<>();
        
        // 处理第一个点（特殊处理）
        FlatCarData firstPoint = points.get(0);
        FlatCarData secondPoint = points.get(1);
        long firstInterval = secondPoint.getTimestamp() - firstPoint.getTimestamp();
        
        // 判断第一个点是否可能有效
        if (isFirstPointValid(firstPoint, secondPoint, firstInterval)) {
            filtered.add(firstPoint);
        }
        // 第二个点总是加入（作为基准）
        filtered.add(secondPoint);

        // 处理后续点
        for (int i = 2; i < points.size(); i++) {
            FlatCarData current = points.get(i);
            
            // 必须至少有2个历史点才能验证
            if (filtered.size() >= 2) {
                FlatCarData prev1 = filtered.get(filtered.size() - 1);
                FlatCarData prev2 = filtered.get(filtered.size() - 2);
                
                if (isPointValid(current, prev1, prev2)) {
                    filtered.add(current);
                }
                // 不满足条件的点直接剔除
            } else {
                // 不足2个历史点时，只做简单距离检查
                FlatCarData last = filtered.get(filtered.size() - 1);
                if (calculateDistance(last, current) <= MAX_DISPLACEMENT) {
                    filtered.add(current);
                }
            }
        }
        
        return filtered;
    }
    
    /**
     * 验证第一个点是否有效
     */
    private static boolean isFirstPointValid(FlatCarData first, FlatCarData second, long timeDiffMs) {
        double distance = calculateDistance(first, second);
        double timeDiffSec = timeDiffMs / 1000.0;
        
        // 冷启动情况
        if (timeDiffMs > COLD_START_THRESHOLD) {
            return distance <= COLD_START_MAX_DISPLACEMENT;
        }
        // 正常情况
        return (distance / timeDiffSec) <= MAX_SPEED;
    }
    
    /**
     * 验证新点是否有效（基于前两个有效点）
     */
    private static boolean isPointValid(FlatCarData current, FlatCarData prev1, FlatCarData prev2) {
        // 计算与前一个点的距离和时间
        double dist1 = calculateDistance(prev1, current);
        double time1 = (current.getTimestamp() - prev1.getTimestamp()) / 1000.0;
        double speed1 = dist1 / time1;
        
        // 计算与前前个点的距离和时间
        double dist2 = calculateDistance(prev2, current);
        double time2 = (current.getTimestamp() - prev2.getTimestamp()) / 1000.0;
        double speed2 = dist2 / time2;
        
        // 双重验证（必须同时满足）
        boolean speedValid = (speed1 <= MAX_SPEED) && (speed2 <= MAX_SPEED * 1.5);
        boolean displacementValid = (dist1 <= MAX_DISPLACEMENT) && (dist2 <= MAX_DISPLACEMENT * 2);
        
        // 方向一致性检查（可选）
//        boolean directionValid = isDirectionConsistent(prev2, prev1, current);
        
        return speedValid && displacementValid;
    }
    
    /**
     * 方向一致性检查（防止急转弯）
     */
    private static boolean isDirectionConsistent(FlatCarData p1, FlatCarData p2, FlatCarData p3) {
        // 计算两个向量的方向变化
        double bearing1 = calculateBearing(Double.valueOf(p1.getLatitude()), Double.valueOf(p1.getLongitude()), Double.valueOf(p2.getLatitude()), Double.valueOf(p2.getLongitude()));
        double bearing2 = calculateBearing(Double.valueOf(p2.getLatitude()), Double.valueOf(p2.getLongitude()), Double.valueOf(p3.getLatitude()), Double.valueOf(p3.getLongitude()));
        
        double angleDiff = Math.abs(bearing2 - bearing1);
        angleDiff = angleDiff > 180 ? 360 - angleDiff : angleDiff;
        
        return angleDiff < 90; // 允许最大90度方向变化
    }
    
    /**
     * 计算两点间距离（米）
     */
    private static double calculateDistance(FlatCarData p1, FlatCarData p2) {
        return calculateDistance(Double.valueOf(p1.getLatitude()), Double.valueOf(p1.getLongitude()), Double.valueOf(p2.getLatitude()), Double.valueOf(p2.getLongitude()));
    }
    
    private static double calculateDistance(double lat1, double lon1, double lat2, double lon2) {
        double dLat = Math.toRadians(lat2 - lat1);
        double dLon = Math.toRadians(lon2 - lon1);
        
        double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
                   Math.cos(Math.toRadians(lat1)) * Math.cos(Math.toRadians(lat2)) *
                   Math.sin(dLon / 2) * Math.sin(dLon / 2);
        
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
        
        return EARTH_RADIUS * c;
    }
    
    /**
     * 计算方位角（度）
     */
    private static double calculateBearing(double lat1, double lon1, double lat2, double lon2) {
        double lat1Rad = Math.toRadians(lat1);
        double lat2Rad = Math.toRadians(lat2);
        double dLonRad = Math.toRadians(lon2 - lon1);
        
        double y = Math.sin(dLonRad) * Math.cos(lat2Rad);
        double x = Math.cos(lat1Rad) * Math.sin(lat2Rad) -
                   Math.sin(lat1Rad) * Math.cos(lat2Rad) * Math.cos(dLonRad);
        
        double bearing = Math.toDegrees(Math.atan2(y, x));
        return (bearing + 360) % 360;
    }

}